#include <FBFlow.h>
#include <matching.h>

void FB::setImageParam(cv::Mat input)
{
  prevGray = Mat::zeros(input.rows, input.cols, CV_8U);
  currGray = Mat::zeros(input.rows, input.cols, CV_8U);
  cvtColor(input, prevGray, CV_RGB2GRAY);
  flow = Mat::zeros(input.rows, input.cols, CV_32FC2);

}

void FB::calcFB(cv::Mat input)
{
  clock_t start, end;

  cvtColor(input, currGray, CV_RGB2GRAY);
  start = clock();
  calcOpticalFlowFarneback(prevGray, currGray, flow, 0.5, 1, 10, 20, 5, 1.1, OPTFLOW_FARNEBACK_GAUSSIAN);
  end = clock();
  currGray.copyTo(prevGray);
  //printf("%.2f秒かかりました\n",(double)(end-start)/CLOCKS_PER_SEC);

}

void FB::getFlowPoint(vector<Point2f> *flowPoint)
{
  if(flowPoint->size() > 0)
    flowPoint->clear();

  Point2f temp;
  for(int i = 0; i<flow.rows; i+=step)
    {
      for(int j = 0; j<flow.cols; j+=step)
	{
	  temp.x = j;
	  temp.y = i;
	  flowPoint->push_back(temp);
	}
    }
}

void FB::getFlowScale(vector<Point2f> *flowScale)
{
  if(flowScale->size() > 0)
    flowScale->clear();

  Point2f temp;
  for(int i = 0; i<flow.rows; i+=step)
    {
      const float* fptr = flow.ptr<float>(i);
      for(int j = 0; j<flow.cols; j+=step)
	{
	  float rad = hypot(fptr[2*j], fptr[2*j+1]);
	  if(rad != 0)
	    {
	      temp.x = fptr[2*j];///rad;
	      temp.y = fptr[2*j + 1];///rad;
	      flowScale->push_back(temp);
	    }
	  else
	    {
	      temp.x = fptr[2*j];
	      temp.y = fptr[2*j + 1];
	      flowScale->push_back(temp);
	    }
	}
    }
}

void FB::firstProc(cv::Mat srcimage)
{
  setImageParam(srcimage);
}

void FB::Proc(std::string dirname, int imcount, cv::Mat& output, vector<Point2f> &flowPoint, vector<Point2f> &flowScale)
{
  static char *home = getenv("HOME");

  //ターゲットの設定
  string imdir;
  imdir.append(home);
  imdir.append("/Pictures/");
  imdir.append(dirname.c_str());
  imdir.append("/");

  output = loadImage(imdir, imcount);

  //resizeSrcImg(output,output, 0.8, INTER_LINEAR);
  resizeSrcImg(output,output, 0.7, INTER_LINEAR);

  calcFB(output);

  getFlowPoint(&flowPoint);
  getFlowScale(&flowScale);

}
